#include <Servo.h>
#include <AsyncTimer.h>
#include "HandleCmdCodec.h"
#include "Motor.h"
#include "ServoChief.h"

//#define DEBUG_MODE
const char *kVersion = "1.1.0";
const uint8_t kMotorPwmPin = 5;
const uint8_t kMotorCtrlPin = 4;
const uint8_t kServoPin = 9;
const uint8_t kLedPin = LED_BUILTIN;

int ledOn = LOW;
HandleCmd cmd;
HandleCmdCodec handleCmdCodec;
Motor motor;
AsyncTimer asynTimer;
ServoChief servoChief;

void printVersion()
{
  Serial.print("version: ");
  Serial.println(kVersion);
}

void onTimeout()
{
  if(cmd) {
    if(ledOn == LOW)
      ledOn = HIGH;
    else
      ledOn = LOW;
  }
  else
    ledOn = LOW;
  digitalWrite(kLedPin, ledOn);
}

void setup() {
  // put your setup code here, to run once:
  pinMode(kLedPin, OUTPUT);
	Serial.begin(9600);
	Serial.setTimeout(100);
  printVersion();
  motor.attach(kMotorPwmPin, kMotorCtrlPin);
  servoChief.attach(kServoPin);
	asynTimer.setInterval(onTimeout, 1000);
}

void loop() {
  // put your main code here, to run repeatedly:
  int speed;
  Motor::MotorMode m;
  cmd = handleCmdCodec.getCmd();
  if(cmd) {
    speed = cmd.motorSpeed();
    m = cmd.motorMode();
    motor.mode(m);
    motor.speedUp(speed);
    //turn
    ServoChief::TurnMode turnMode = cmd.turnMode();
    uint8_t angle = cmd.turnAngle();
    switch(turnMode) {
      case ServoChief::TurnMode::TURN_MODE_LEFT:
        #ifdef DEBUG_MODE
        Serial.print("left: ");
        Serial.println(servoChief.angle());
        #endif
        servoChief.turnLeft(angle);
        break;
      case ServoChief::TurnMode::TURN_MODE_RIGHT:
        #ifdef DEBUG_MODE
        Serial.print("right: ");
        Serial.println(servoChief.angle());
        #endif
        servoChief.turnRight(angle);
        break;
      case ServoChief::TurnMode::TURN_MODE_STANDBY:
      default:
        #ifdef DEBUG_MODE
        Serial.print("mid: ");
        Serial.println(servoChief.angle());
        #endif
        servoChief.returnBack();
        break;
    }
  }
  else {
    motor.shortBrake();
    servoChief.returnBack();
    cmd.reset();
  }
  asynTimer.handle();
}
